package org.team751.subsystems;

import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Relay.Value;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.team751.RobotMap;
import org.team751.commands.ForkliftJoystickControl;
import org.team751.sensors.LimitSwitch;

/**
 * A subsystem for the forklift
 * @author Sam Crow
 */
public class Forklift extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    private Relay relay1;
    private Relay relay2;
    
    private LimitSwitch topLimitSwitch;
    private LimitSwitch bottomLimitSwitch;
    
    public Forklift(){
        relay1 = new Relay(RobotMap.forkliftRelay1);
        relay2 = new Relay(RobotMap.forkliftRelay2);
        
        topLimitSwitch = new LimitSwitch(RobotMap.forkliftTopLimitSwitch);
        bottomLimitSwitch = new LimitSwitch(RobotMap.forkliftBottomLimitSwitch);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new ForkliftJoystickControl());
    }
    
    /**
     * Set the relays to move the forklift up.<br />
     * If the top limit switch is pressed, this is ignored.
     */
    public void setRelaysUp(){
    	if(!topLimitSwitchPressed()){
	        setRelays(Value.kReverse);
    	}else{
    		stop();
    	}
    }
    /**
     * Set the relays to move the forklift down.<br />
     * If the bottom limit switch is pressed, this is ignored.
     */
    public void setRelaysDown(){
    	if(!bottomLimitSwitchPressed()){
        	setRelays(Value.kForward);
    	}else{
    		stop();
    	}
    }
    
    /**
     * Stop the movement of the forklift
     */
    public void stop(){
        setRelays(Value.kOff);
    }
    /**
     * Set both relays
     * @param value the value to set
     */
    protected void setRelays(Value value){
    	relay1.set(value);
    	relay2.set(value);
    }

    /**
     * Get the state of the top limit switch
     * @return True if the top limit switch is pressed, otherwise false.
     */
    public boolean topLimitSwitchPressed(){
        return topLimitSwitch.get();
    }
    /**
     * Get the state of the bottom limit switch
     * @return True if the top limit switch is pressed, false otherwise
     */
    public boolean bottomLimitSwitchPressed(){
    	return bottomLimitSwitch.get();
    }
}
